#include <string.h>
#include "mbrtu.h"
#include "mbtype.h"

int32_t mbrtu_init(mbrtu_type *p,
                   uint8_t id,
                   pcoil_fun coil_callback,
                   preg_fun reg_callback)
{
    if (id == 0xFF)
    {
        MSG_ERR(id, "MBRTU_Init", MBRTU_ERR_ID);
        return MBRTU_ERR_ID;
    }
    memset(p, 0, sizeof(mbrtu_type));
    p->id = id;
    p->coil_callback = coil_callback;
    p->reg_callback = reg_callback;
    return MBRTU_ERR_NONE;
}

int32_t mbrtu_mainlooptcp(mbrtu_type *p, uint8_t *prb, uint8_t *ptb, uint16_t rlen, uint16_t *ptlen)
{
    uint8_t *pcoil, err_no = 0;
    uint16_t datanum, i, addr, *preg;
    mbtcp_t *pmb = (mbtcp_t *)prb;
    mbtcp_t *pmbt = (mbtcp_t *)ptb;
    uint8_t *pr = &prb[6];
    uint8_t *pt = &ptb[6];
    if ((pmb->mbap.node != p->id) || (pmb->mbap.node == 0xFF))
    {
        MSG_ERR(pmb->mbap.node, "MBRTU_ERR_ID", MBRTU_ERR_ID);
        return MBRTU_ERR_ID;
    }
    memcpy(ptb, prb, 4);
    pmbt->mbap.node = p->id;
    switch (pr[1])
    {
    case 1:
    case 2:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5] + 7) / 8;
        if (MBRTU_ERR_NONE != p->coil_callback(&addr, datanum, &pcoil))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        pt[0] = pr[0];
        pt[1] = pr[1];
        pt[2] = datanum;
        for (i = 0; i < datanum * 8; i++)
        {
            if ((i & 7) == 0)
            {
                pt[i / 8 + 3] = 0;
            }
            pt[i / 8 + 3] >>= 1;
            if (i != pr[5])
            {
                pt[i / 8 + 3] |= ((pcoil[(addr + i) / 8] & (1 << ((addr + i) & 7))) == 0) ? 0 : 0x80;
            }
            else
            {
                pt[i / 8 + 3] |= 0;
            }
        }
        *ptlen = 9 + datanum;
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    case 3:
    case 4:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        pt[0] = pr[0];
        pt[1] = pr[1];
        pt[2] = datanum;
        for (i = 0; i < datanum / 2; i++)
        {
            pt[3 + i * 2] = preg[addr + i] >> 8;
            pt[4 + i * 2] = preg[addr + i] & 0xFF;
        }
        *ptlen = 9 + datanum;
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    case 5:
        addr = pr[2] * 0x100 + pr[3];
        if (MBRTU_ERR_NONE != p->coil_callback(&addr, 1, &pcoil))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        memcpy(pt, pr, 8);
        *ptlen = 12;
        if (pr[4] == 0xFF)
        {
            pcoil[addr / 8] |= (uint8_t)1 << (addr & 7);
        }
        else
        {
            pcoil[addr / 8] ^= (uint8_t)1 << (addr & 7);
        }
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    case 6:
        addr = pr[2] * 0x100 + pr[3];
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, 1, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        memcpy(pt, pr, 8);
        *ptlen = 12;
        preg[addr] = (pr[4] << 8) | pr[5];
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    case 15:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->coil_callback(&addr, datanum / 2, &pcoil))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        memcpy(pt, pr, 6);
        *ptlen = 12;
        for (i = 0; i < datanum / 2; i++)
        {
            if (pr[i / 8 + 7] & 1)
            {
                pcoil[(addr + i) / 8] |= (uint8_t)1 << ((addr + i) & 7);
            }
            else
            {
                pcoil[(addr + i) / 8] ^= (uint8_t)1 << ((addr + i) & 7);
            }
            pr[i / 8 + 7] >>= 1;
        }
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    case 16:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        memcpy(pt, pr, 6);
        *ptlen = 12;
        for (i = 0; i < datanum / 2; i++)
            preg[addr + i] = (pr[2 * i + 7] << 8) | pr[2 * i + 8];
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    case 23:
        addr = pr[6] * 0x100 + pr[7];
        // datanum = ( pr[8] * 0x100 + pr[9] )<<1;
        datanum = pr[10];
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        for (i = 0; i < datanum / 2; i++)
            preg[addr + i] = (pr[2 * i + 11] << 8) | pr[2 * i + 12];
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto tcperror;
        }
        for (i = 0; i < datanum / 2; i++)
        {
            pt[3 + i * 2] = preg[addr + i] >> 8;
            pt[4 + i * 2] = preg[addr + i] & 0xFF;
        }
        *ptlen = 9 + datanum;
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_NONE;
    default:
        err_no = ILLEGALFUNCTION;
    tcperror:
        pt[0] = pr[0];
        pt[1] = pr[1] | 0x80;
        pt[2] = err_no;
        *ptlen = 11;
        pmbt->mbap.len = *ptlen - 6;
        pmbt->mbap.len = SWAP16(pmbt->mbap.len);
        return MBRTU_ERR_FUN;
    }
}
int32_t mbrtu_mainloop(mbrtu_type *p, uint8_t *pr, uint8_t *pt, uint16_t rlen, uint16_t *ptlen)
{
    uint8_t *pcoil, err_no = 0;
    uint16_t datanum, i, crc, addr, rcrc, *preg, byte_num;

    if (rlen < 5)
    {
        MSG_ERR(rlen, "MBRTU_ERR_RLEN", MBRTU_ERR_RLEN);
        return MBRTU_ERR_RLEN;
    }
    if ((pr[0] != p->id) || (pr[0] == 0xFF))
    {
        MSG_ERR(pr[0], "MBRTU_ERR_ID", MBRTU_ERR_ID);
        return MBRTU_ERR_ID;
    }
    rcrc = pr[rlen - 2] * 0x100 + pr[rlen - 1];
    crc = mbrtu_crc16(pr, rlen - 2);
    if (crc != rcrc)
    {
        MSG_ERR(crc, "MBRTU_ERR_CRC", rcrc);
        return MBRTU_ERR_CRC;
    }
    switch (pr[1])
    {
    case 1:
    case 2:
        addr = pr[2] * 0x100 + pr[3];
        byte_num = (pr[4] * 0x100 + pr[5] + 7) / 8;
        datanum = pr[4] * 0x100 + pr[5];
        if (MBRTU_ERR_NONE != p->coil_callback(&addr, datanum, &pcoil))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        pt[0] = pr[0];
        pt[1] = pr[1];
        pt[2] = byte_num;
        for (i = 0; i < datanum; i++)
        {
            if ((i & 7) == 0)
            {
                pt[i / 8 + 3] = 0;
            }
            pt[i / 8 + 3] >>= 1;
            if (i != pr[5])
            {
                pt[i / 8 + 3] |= ((pcoil[(addr + i) / 8] & (1 << ((addr + i) & 7))) == 0) ? 0 : 0x80;
            }
            else
            {
                pt[i / 8 + 3] |= 0;
            }
        }
        crc = mbrtu_crc16(pt, 3 + byte_num);
        *ptlen = 5 + byte_num;
        pt[byte_num + 3] = crc / 0x100;
        pt[byte_num + 4] = crc & 0xff;
        return MBRTU_ERR_NONE;
    case 3:
    case 4:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        pt[0] = pr[0];
        pt[1] = pr[1];
        pt[2] = datanum;
        for (i = 0; i < datanum / 2; i++)
        {
            pt[3 + i * 2] = preg[addr + i] >> 8;
            pt[4 + i * 2] = preg[addr + i] & 0xFF;
        }
        crc = mbrtu_crc16(pt, 3 + datanum);
        *ptlen = 5 + datanum;
        pt[datanum + 3] = crc >> 8;
        pt[datanum + 4] = crc & 0xFF;
        return MBRTU_ERR_NONE;
    case 5:
        addr = pr[2] * 0x100 + pr[3];
        if (MBRTU_ERR_NONE != p->coil_callback(&addr, 1, &pcoil))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        memcpy(pt, pr, 8);
        *ptlen = 8;
        if (pr[4] == 0xFF)
        {
            pcoil[addr / 8] |= (uint8_t)1 << (addr & 7);
        }
        else
        {
            pcoil[addr / 8] ^= (uint8_t)1 << (addr & 7);
        }
        return MBRTU_ERR_NONE;
    case 6:
        addr = pr[2] * 0x100 + pr[3];
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, 1, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        memcpy(pt, pr, 8);
        *ptlen = 8;
        preg[addr] = (pr[4] << 8) | pr[5];
        return MBRTU_ERR_NONE;
    case 15:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->coil_callback(&addr, datanum / 2, &pcoil))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        memcpy(pt, pr, 6);
        crc = mbrtu_crc16(pt, 6);
        pt[6] = crc >> 8;
        pt[7] = crc & 0xFF;
        *ptlen = 8;
        for (i = 0; i < datanum / 2; i++)
        {
            if (pr[i / 8 + 7] & 1)
            {
                pcoil[(addr + i) / 8] |= (uint8_t)1 << ((addr + i) & 7);
            }
            else
            {
                pcoil[(addr + i) / 8] ^= (uint8_t)1 << ((addr + i) & 7);
            }
            pr[i / 8 + 7] >>= 1;
        }
        return MBRTU_ERR_NONE;
    case 16:
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        memcpy(pt, pr, 6);
        crc = mbrtu_crc16(pt, 6);
        pt[6] = crc >> 8;
        pt[7] = crc & 0xFF;
        *ptlen = 8;
        for (i = 0; i < datanum / 2; i++)
            preg[addr + i] = (pr[2 * i + 7] << 8) | pr[2 * i + 8];
        return MBRTU_ERR_NONE;
    case 23:
        addr = pr[6] * 0x100 + pr[7];
        // datanum = ( pr[8] * 0x100 + pr[9] )<<1;
        datanum = pr[10];
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        for (i = 0; i < datanum / 2; i++)
            preg[addr + i] = (pr[2 * i + 11] << 8) | pr[2 * i + 12];
        addr = pr[2] * 0x100 + pr[3];
        datanum = (pr[4] * 0x100 + pr[5]) << 1;
        if (MBRTU_ERR_NONE != p->reg_callback(&addr, datanum / 2, &preg))
        {
            err_no = ILLEGALDATAADDRESS;
            goto error;
        }
        pt[0] = pr[0];
        pt[1] = pr[1];
        pt[2] = datanum;
        for (i = 0; i < datanum / 2; i++)
        {
            pt[3 + i * 2] = preg[addr + i] >> 8;
            pt[4 + i * 2] = preg[addr + i] & 0xFF;
        }
        crc = mbrtu_crc16(pt, 3 + datanum);
        *ptlen = 5 + datanum;
        pt[datanum + 3] = crc >> 8;
        pt[datanum + 4] = crc & 0xFF;
        return MBRTU_ERR_NONE;
    default:
        err_no = ILLEGALFUNCTION;
    error:
        pt[0] = pr[0];
        pt[1] = pr[1] | 0x80;
        pt[2] = err_no;
        crc = mbrtu_crc16(pt, 3);
        pt[3] = crc >> 8;
        pt[4] = crc & 0xFF;
        *ptlen = 5;
        return MBRTU_ERR_FUN;
    }
}

#if 1
uint16_t mbrtu_crc16(uint8_t *pBuf, uint16_t dataLen)
{
    typedef union
    {
        uint16_t val;
        struct
        {
            uint16_t bit0 : 1;
            uint16_t bit1 : 1;
            uint16_t bit2 : 1;
            uint16_t bit3 : 1;
            uint16_t bit4 : 1;
            uint16_t bit5 : 1;
            uint16_t bit6 : 1;
            uint16_t bit7 : 1;
            uint16_t bit8 : 1;
            uint16_t bit9 : 1;
            uint16_t bit10 : 1;
            uint16_t bit11 : 1;
            uint16_t bit12 : 1;
            uint16_t bit13 : 1;
            uint16_t bit14 : 1;
            uint16_t bit15 : 1;
        } bits;
    } CRCReg;
    int i;
    uint8_t j, nTest;
    CRCReg reg;

    reg.val = 0xFFFF;
    for (i = 0; i < dataLen; i++)
    {
        reg.val ^= *pBuf++;
        for (j = 0; j < 8; j++)
        {
            nTest = (reg.bits.bit0) ? 0x01 : 0x00;
            reg.val >>= 1;
            if (nTest == 1)
                reg.val ^= 0xA001;
        }
    }
    reg.val = (((reg.val >> 8)) + ((reg.val) << 8));
    return reg.val;
}
#else
const uint16_t wCRCTable[] = {
    0X0000, 0XC0C1, 0XC181, 0X0140, 0XC301, 0X03C0, 0X0280, 0XC241,
    0XC601, 0X06C0, 0X0780, 0XC741, 0X0500, 0XC5C1, 0XC481, 0X0440,
    0XCC01, 0X0CC0, 0X0D80, 0XCD41, 0X0F00, 0XCFC1, 0XCE81, 0X0E40,
    0X0A00, 0XCAC1, 0XCB81, 0X0B40, 0XC901, 0X09C0, 0X0880, 0XC841,
    0XD801, 0X18C0, 0X1980, 0XD941, 0X1B00, 0XDBC1, 0XDA81, 0X1A40,
    0X1E00, 0XDEC1, 0XDF81, 0X1F40, 0XDD01, 0X1DC0, 0X1C80, 0XDC41,
    0X1400, 0XD4C1, 0XD581, 0X1540, 0XD701, 0X17C0, 0X1680, 0XD641,
    0XD201, 0X12C0, 0X1380, 0XD341, 0X1100, 0XD1C1, 0XD081, 0X1040,
    0XF001, 0X30C0, 0X3180, 0XF141, 0X3300, 0XF3C1, 0XF281, 0X3240,
    0X3600, 0XF6C1, 0XF781, 0X3740, 0XF501, 0X35C0, 0X3480, 0XF441,
    0X3C00, 0XFCC1, 0XFD81, 0X3D40, 0XFF01, 0X3FC0, 0X3E80, 0XFE41,
    0XFA01, 0X3AC0, 0X3B80, 0XFB41, 0X3900, 0XF9C1, 0XF881, 0X3840,
    0X2800, 0XE8C1, 0XE981, 0X2940, 0XEB01, 0X2BC0, 0X2A80, 0XEA41,
    0XEE01, 0X2EC0, 0X2F80, 0XEF41, 0X2D00, 0XEDC1, 0XEC81, 0X2C40,
    0XE401, 0X24C0, 0X2580, 0XE541, 0X2700, 0XE7C1, 0XE681, 0X2640,
    0X2200, 0XE2C1, 0XE381, 0X2340, 0XE101, 0X21C0, 0X2080, 0XE041,
    0XA001, 0X60C0, 0X6180, 0XA141, 0X6300, 0XA3C1, 0XA281, 0X6240,
    0X6600, 0XA6C1, 0XA781, 0X6740, 0XA501, 0X65C0, 0X6480, 0XA441,
    0X6C00, 0XACC1, 0XAD81, 0X6D40, 0XAF01, 0X6FC0, 0X6E80, 0XAE41,
    0XAA01, 0X6AC0, 0X6B80, 0XAB41, 0X6900, 0XA9C1, 0XA881, 0X6840,
    0X7800, 0XB8C1, 0XB981, 0X7940, 0XBB01, 0X7BC0, 0X7A80, 0XBA41,
    0XBE01, 0X7EC0, 0X7F80, 0XBF41, 0X7D00, 0XBDC1, 0XBC81, 0X7C40,
    0XB401, 0X74C0, 0X7580, 0XB541, 0X7700, 0XB7C1, 0XB681, 0X7640,
    0X7200, 0XB2C1, 0XB381, 0X7340, 0XB101, 0X71C0, 0X7080, 0XB041,
    0X5000, 0X90C1, 0X9181, 0X5140, 0X9301, 0X53C0, 0X5280, 0X9241,
    0X9601, 0X56C0, 0X5780, 0X9741, 0X5500, 0X95C1, 0X9481, 0X5440,
    0X9C01, 0X5CC0, 0X5D80, 0X9D41, 0X5F00, 0X9FC1, 0X9E81, 0X5E40,
    0X5A00, 0X9AC1, 0X9B81, 0X5B40, 0X9901, 0X59C0, 0X5880, 0X9841,
    0X8801, 0X48C0, 0X4980, 0X8941, 0X4B00, 0X8BC1, 0X8A81, 0X4A40,
    0X4E00, 0X8EC1, 0X8F81, 0X4F40, 0X8D01, 0X4DC0, 0X4C80, 0X8C41,
    0X4400, 0X84C1, 0X8581, 0X4540, 0X8701, 0X47C0, 0X4680, 0X8641,
    0X8201, 0X42C0, 0X4380, 0X8341, 0X4100, 0X81C1, 0X8081, 0X4040};
uint16_t mbrtu_crc16(uint8_t *nData, uint16_t wLength)
{
    uint8_t nTemp;
    uint16_t wCRCWord = 0xFFFF;

    while (wLength--)
    {
        nTemp = *nData++ ^ wCRCWord;
        wCRCWord >>= 8;
        wCRCWord ^= wCRCTable[nTemp];
    }
    return wCRCWord;

}

#endif
